import edu.wsu.KheperaSimulator.RobotController;

/**
 * 
 * The Controller3a class. The robots runs toward a light bulb and runs into it.
 * Bang-bang controller
 * 
 * @author Philipp Rusch und Markus Wetzel
 */
public class Controller4b extends RobotController {

	// Sensoren:
	// 0 .. Links
	// 1 .. Links Vorne Schraeg
	// 2 .. links Vorne
	// 3 .. Rechts Vorne
	// 4 .. Rechts Vorne Schraeg
	// 5 .. Rechts
	// 6 .. Rechts Hinten
	// 7 .. Links Hinten

	@Override
	public void close() throws Exception {
	}

	@Override
	public void doWork() throws Exception {
		int left = 4;
		int right = 4;
		int check1 = 400;
		int check2 = 200;

		// Vorne - halb links
		if (getLightValue(1) < check1) {
			left -= 1;
		}
		// Vorne links
		if (getLightValue(2) < check1) {
			left -= 1;
		}
		// Vorne rechts
		if (getLightValue(3) < check1) {
			right -= 1;
		}
		// Vorne halb rechts
		if (getLightValue(4) < check1) {
			right -= 1;
		}
		// Vorne - halb links

		if (getLightValue(1) < check2) {
			left -= 1;
		}
		// Vorne links

		if (getLightValue(2) < check2) {
			left -= 1;
		}
		// Vorne rechts

		if (getLightValue(3) < check2) {
			right -= 1;
		}
		// Vorne halb rechts

		if (getLightValue(4) < check2) {
			right -= 1;
		}

		setMotorSpeeds(left, right);

	}
}
